#ifndef __DEADLOCK_HANDLER__
#define __DEADLOCK_HANDLER__

#include <dispatch_system/moying_map.h>
#include <vector>
#include <moying_proto/moying_proto.h>

namespace moying
{
namespace navigation
{

//路径规划状态
enum PathPlanState{
    kPathPlanInit=0,        //初始状态
    kWaitPathPlanResult,    //等待路径规划结果
    kWaitNavArrive,         //等待导航到达
    kNavArrived,            //导航到达
    kPathPlanFailed,        //规划失败
    kNavFailed              //导航失败
};

//冲突区域
struct ConflictArea{
    moying_proto::Pose2D area_coor;       //冲突区域中心坐标
    float dis_tolerance;    //死锁机器人距离冲突区域中心距离小于该值,则认为机器人属于该冲突区
    std::vector<moying_proto::Pose2D>  custom_avoid_pts;  //自定义的避让点
    bool only_use_custom;   //是否只使用自定义的冲突避让点
};

//死锁的机器人处理信息
struct DeadLockRobotHandleInfo{
    VehiclePtr robot = nullptr;                       //机器人指针
    std::vector<VehicleState> temp_goals;   //机器人脱离开死锁的附近导航点数组,数组已根据到死锁机器人组的总距离排序,从前往后顺序尝试导航点
    int next_pos_index          = 0;        //下一个尝试的附近导航点索引 
    bool solved                 = false;    //是否解决了死锁，路径规划成功
    bool retry_global_path_plan = false;    //是否在重新尝试全局路径规划中
    bool avoid_handled          = false;    //是否进行过避让处理（导航到临时避让点成功，或者所有临时目标点都导航失败）
    PathPlanState path_plan_state     = kPathPlanInit;  //临时避让的路径规划状态          
    int robot_weight            = 0;        //临时避让时选择优先级权重
    int avoid_area_minx         = 0;        //避让范围的最小x坐标
    int avoid_area_miny         = 0;        //避让范围的最小y坐标    
    int avoid_area_maxx         = 0;        //避让范围的最大x坐标
    int avoid_area_maxy         = 0;        //避让范围的最大y坐标
};

//死锁机器人组
struct DeadlockRobotGroup{
    std::vector<DeadLockRobotHandleInfo> deadlock_robots;   //死锁机器人
    int next_try_robot_index;   //下一个尝试解决死锁的机器人索引 
    int group_id;   //死锁组id
    int begin_wait_time;    //开始等待解决冲突路径规划或者导航到达的时间 
    ConflictArea* custom_conflict_area;   //自定义冲突区域
};


//机器人死锁处理器类
class DeadLockHandler{

public:

    DeadLockHandler(MoyingMapPtr moying_map);

    /**
     * @brief 处理死锁情况
     */ 
    void handleDeadLock();

    /**
     * @brief 判断机器人是否死锁
     * @param vehicle   机器人对象
     * @return true：死锁，false：没有死锁
     */ 
    bool isRobotInDeadLockGroup(VehiclePtr vehicle);

    /**
     * @brief 更新路径规划结果
     * @param vehicle   机器人对象
     * @param result    路径规划结果，true:成功，false:失败 
     * @param goal      目标点
     */ 
    void updatePathPlanResult(VehiclePtr vehicle,bool result,VehicleState goal);

    /**
     * @brief 处理机器人到达通知
     * @return 处理成功或失败
     */ 
    bool handleNavStateNotify(moying_proto::NavStateNotify notify);

    /**
     * @brief 处理机器人停止导航通知
     * @return 
     */ 
    void handleRobotStopNav(unsigned int robot_id);

    /**
     * @brief 把机器人从死锁组中删除
     * @param robot_id  机器人id
     * @return 处理成功或失败
     */ 
    bool removeRobotFromDeadlockGroup(unsigned int robot_id);

private:

    bool init();

    /**
     * @brief 给死锁的机器人分组
     */ 
    void groupDeadLockRobot();

    /**
     * @brief 计算死锁机器人的临时目标点
     */ 
    void calculateTempGoal();

    /**
     * @brief 选择机器人进行局部避让处理
     * @param group 要避让处理的死锁组
     * @return true:有机器人可以进行避让处理,false:没有机器人可以进行避让处理
     */ 
    bool selectRobotToAvoid(DeadlockRobotGroup& group);

    /**
     * @brief 重新尝试全局路径规划
     * @param group 机器人死锁组
     * @param path_plan_delay_seconds   尝试路径规划的时间
     * @param request_all   true:死锁组中对全部没有全局规划成功的机器人请求一次路径规划,false:只对符合条件的机器人请求全局路径规划
     * @return 返回尝试路径规划的机器人数量
     */ 
    int retryGlobalPathPlan(DeadlockRobotGroup& group,int path_plan_delay_seconds=0,bool request_all=false);
    

private:
    static int next_group_id;
    std::recursive_mutex group_mutex_;
    MoyingMapPtr moying_map_=nullptr;
    std::vector<DeadlockRobotGroup> deadlock_robot_group_;  //死锁机器人组
    std::vector<ConflictArea>   custom_conflict_area_;  //自定义的冲突区
};


}
}

#endif